/*
 *  test.h
 *
 *  David Lobato 2011 <dav.lobato@gmail.com>
 *
 *  This file is part of Surveyor SVS player/stage driver.
 *
 *  Surveyor SVS player/stage driver is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *   Surveyor SVS player/stage driver is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with Surveyor SVS player/stage driver.
 *   If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "surveyorsvs.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>


int main(int argc, char* argv[]){
	svs_comm_t* r;

	if (argc != 4){
		printf("usage: test ip master_port slave_port\n");
		exit(-1);
	}

	r = svs_create(argv[1],atoi(argv[2]),atoi(argv[3]));
	svs_init(r);
	printf("battery status: %d\n",svs_get_battery_status(r));

	//servo 1 functions. On renegade: left=v right=w
	svs_set_servo1_max_values(&r->modules[SVS_MASTER],0.5,0.5);
	svs_set_servo1_values(&r->modules[SVS_MASTER],0.0,0.0);
	usleep(1000000);
	svs_set_servo1_values(&r->modules[SVS_MASTER],1.0,0.0);
	usleep(1000000);
	svs_set_servo1_values(&r->modules[SVS_MASTER],0.0,0.0);

	//servo 2 functions
	svs_set_servo2_max_values(&r->modules[SVS_MASTER],0.5,0.5);
	svs_set_servo2_values(&r->modules[SVS_MASTER],0.0,0.0);
	usleep(1000000);
	svs_set_servo2_values(&r->modules[SVS_MASTER],1.0,1.0);
	usleep(1000000);
	svs_set_servo2_values(&r->modules[SVS_MASTER],-1.0,-1.0);
	usleep(1000000);
	svs_set_servo2_values(&r->modules[SVS_MASTER],0.0,0.0);

	//get images from master and slave
	svs_set_image_mode(&r->modules[SVS_MASTER], RGB888_320x240);
	svs_get_image(&r->modules[SVS_MASTER]);
	FILE *image_fd;
	image_fd = fopen("master.rgb","w");
	fwrite((void*)r->modules[SVS_MASTER].image_buf,sizeof(char),
			r->modules[SVS_MASTER].image_buf_size,image_fd);
	fclose(image_fd);

	svs_set_image_mode(&r->modules[SVS_SLAVE], RGB888_320x240);
	svs_get_image(&r->modules[SVS_SLAVE]);
	image_fd = fopen("slave.rgb","w");
	fwrite((void*)r->modules[SVS_SLAVE].image_buf,sizeof(char),
			r->modules[SVS_SLAVE].image_buf_size,image_fd);
	fclose(image_fd);

	svs_close(r);
	svs_destroy(r);
}
